#include "turner.h"
#include "string.h"
#include <stdlib.h>
#include "aos/kv.h"
#include "../common/common_motor.h"
#include "../common/device_manager.h"
#include "../../bsp/bsp.h"
#include "hcfa.h"

struct xp_turner {
    u8 inited;          //init state
    u8 check_zero;      //check zero state
    u8 run;             //turner run state
    int error;
    struct {
        int origin;
        int current;
        int next;
    } motor_angle_offset; // 折算为电机旋转角度信息
    struct {
        float max;
        float min;
        float current;
        float next;
    } angle_offset;    // 举平高度信息
    float move_speed;   //turner 度/s
    int warn_code;
    u8 speed_block;      //speed block number
    struct xp_motor motor;  //turner motor
    aos_mutex_t lock;
};

#define TURNER_ORIGIN_ANGLE_OFFSET_KEY "turner.origin"

static int xp_turner_check_warn(void);
static int xp_turner_get_location(void);

static struct xp_turner turner={0};

static u8 move_quit=0;

static xp_bool local_config_kv_get(const char *key, char *value, int *length)
{
    int retry_count = 0;
    for (retry_count = 0; retry_count < 5; retry_count ++) {
        if (0 == aos_kv_get(key, value, length)) {
            return xp_true;
        }
    }
    if (retry_count >= 5) return xp_false;
}

static xp_bool local_config_kv_set(const char *key, const char *value)
{
    return aos_kv_set(key, value, strlen(value), 1) == 0 ? xp_true : xp_false;
}

static int xp_turner_current_angle_offset_get()
{
    int num_of_turns = 0, sub_angle = 0;

    if(turner.motor.ops->encode_get(&turner.motor, &num_of_turns, &sub_angle) != 0){
        println("error:get turner angle offset fail~");
        return -1;
    }
    turner.motor_angle_offset.current = num_of_turns * 360 + sub_angle;
    return 0;
}

static void xp_turner_read_kv()
{
    char value[20] = {0};
    int length = 20;

    if (xp_true == local_config_kv_get(TURNER_ORIGIN_ANGLE_OFFSET_KEY, value, &length)) {
        turner.motor_angle_offset.origin = atoi(value);
        println("aos_kv_get turner.origin %s", value);
    } else {
        xp_turner_current_angle_offset_get();
        turner.motor_angle_offset.origin = turner.motor_angle_offset.current;
        println("aos_kv_get turner.origin failed! use current turner position as zero!");
    }
    turner.angle_offset.max = 100;
    turner.angle_offset.min = -10;
}

static xp_bool xp_turner_top_limit_touched()
{
    if (xp_io_read_input_pin(1, TURNERS_io_top) == TURNERS_io_touch) {
        println("turner top limit touched.");
        return xp_true;
    }
    return xp_false;
}

static xp_bool xp_turner_bottom_limit_touched()
{
    if (xp_io_read_input_pin(1, TURNERS_io_down) == TURNERS_io_touch) {
        println("turner bottom limit touched.");
        return xp_true;
    }
    return xp_false;
}

static int xp_turner_move_time_estimate(float angle)
{
    float move_angle = angle - turner.angle_offset.current;
    if (move_angle < 0) move_angle = 0 - move_angle;
    float time = move_angle / turner.move_speed;       // 移动角度单位是度 速度单位是rpm 即时间是秒
    return (int)(time * 1000 + 2000);   // 转换为微秒，并加上2秒的加减速预留时间
}

static int xp_turner_motor_rotation_angle_get(float angle)
{
    int motor_angle = 0;
    // 转动72度为行走0.1度
    motor_angle = angle * TURNERS_ratio;
    return motor_angle;
}

static int xp_turner_angle_set(float angle)
{
    if (turner.inited == 0) return -1;
    int ret = 0;
    float move_angle = angle - turner.angle_offset.current;
    int dir = (move_angle >= 0) ? 1 : -1;      // 1表示向上移动    -1表示向下移动
    move_angle = (move_angle >= 0) ? move_angle : 0 - move_angle;   // 取绝对值
    if (move_angle < 0.08) return 0;      // 小于0.08视为0, 保证一定的误差下任有0.1的移动精度
    int motor_angle = xp_turner_motor_rotation_angle_get(move_angle);
    int num_of_turn = motor_angle / 360;
    int sub_angle = motor_angle % 360;
    turner.motor_angle_offset.next = (dir == 1) ? turner.motor_angle_offset.current + motor_angle : turner.motor_angle_offset.current - motor_angle;
    ret |= turner.motor.ops->position_set(&turner.motor, num_of_turn, sub_angle);
    ret |= turner.motor.ops->direction_set(&turner.motor, (dir >= 0) ? motor_forward : motor_reverser);

    return (ret == 0) ? 0 : -2;
}

/**
 * @brief 举平初始化
 * 
 * @return int 返回0表示成功，返回其他值表示失败
 */
int xp_turners_init()
{
    // 获取举平电机对象
    struct xp_modbus_device *dev = get_modbus_device_by_sid(1);
    xp_hcfa_position_motor_constructor(&turner.motor, dev, (unsigned char *)"turner_motor");
    if (turner.inited) return -1;
    if (turner.motor.inited != 1) return -2;

    xp_turner_read_kv();

    xp_turner_change_speed(TURNERS_angle_speed);

    xp_turner_get_location();

    turner.inited = 1;
    return 0;
}

/**
 * @brief 清除举平电机的报警
 * 
 * @return int 返回0表示成功，返回其他值表示失败
 */
int xp_clear_turner_warn()
{
    int ret = turner.motor.ops->error_clear(&turner.motor);
    return (ret == 0) ? 0 : -1;
}

/**
 * @brief 改变举平的旋转速度
 * 
 * @param angle_speed 待改变的目标旋转速度，单位是度/s
 * @return int 返回0表示成功，返回其他值表示失败
 */
int xp_turner_change_speed(float angle_speed)
{
    int ret = -1;
    int motor_speed_rpm = 0;

    turner.move_speed = angle_speed;

    motor_speed_rpm = xp_turner_convert_speed(angle_speed);
    ret = turner.motor.ops->speed_set(&turner.motor, motor_speed_rpm);
    if(ret != 0) {
        println("change_speed %d failed! ret %d", motor_speed_rpm, ret);
        return ret;
    }
    return 0;
}

// 举平电机的旋转速度转换为实际电机的rpm转速
int xp_turner_convert_speed(u8 angle_speed){
    return angle_speed*TURNERS_ratio/6;
}

int xp_clear_turners_warn(void){
    gpio_dev_t gpio;
	uint32_t turner_err = 0;
    u8 clear_warn_try_again_cnt;

    //检测是否清成功，不成功重试
    clear_warn_try_again_cnt = 5;
    while(clear_warn_try_again_cnt--)
    {
        aos_msleep(50);
        if(turner.motor.ops->rotation_stop(&turner.motor)!=0)
        {
            println("turners enable off faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(turner.motor.ops->error_clear(&turner.motor) != 0)
        {
            println("turners warn clear faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(turner.motor.ops->rotation_start(&turner.motor) != 0)
        {
            println("turners enable on faild!!!!!!!!");
            continue;
        }

        //举平驱动器错误检测
        gpio.port = 109;
        hal_gpio_input_get(&gpio, &turner_err);
        if(1 == turner_err)
        {
            aos_msleep(200);
        }
        else
        {
            xp_turner_get_location();//更新机构的location
            println("turnner warn clear success! try cnt = %d",(5-clear_warn_try_again_cnt));
            xp_io_write_pin(1,14,0);                //松开举平抱闸
            return 0;
        }
    }

    println("turners warn clear faild!!!");
    return -1;
}

// 停止举平动作
static int xp_turner_move_stop()
{
    if (turner.inited == 0) return -1;
    int ret = turner.motor.ops->rotation_stop(&turner.motor);

    return ret;
}

// 开始举平动作
static int xp_turner_move_start()
{
    if (turner.inited == 0) return -1;
    int ret = turner.motor.ops->rotation_start(&turner.motor);

    return ret;
}

// 举平移动支持重试接口
int xp_turners_move_support_retry(s8 angle,u8 speed_block,u8 retry_cnt){
    int ret = -1;
    u8 retry_num = 0;

    retry_num = retry_cnt;
    ret = xp_turners_move(angle,speed_block);
    while(retry_num--){
        if(ret != 0){
            aos_msleep(200);
            println("turners move failed, retry num %d", retry_num);
            ret = xp_turners_move(angle,speed_block);
        }
        else{
            return ret;
        }
    }
    if(ret != 0){
        println("turners move failed, retry cnt %d", retry_cnt);
    }
    return ret;
}

/**
 * @brief 举平旋转控制主要函数
 * 
 * @param angle 期望旋转到的角度
 * @param speed_block 这个在新电机的程序里面为用到，这里是为了接口兼容老的程序而保留，实际没用
 * @return int 返回0表示成功，返回其他值表示失败
 */
int xp_turners_move(s8 angle,u8 speed_block)
{

    if(turner.inited == 0) {
        println("turner not inited.");
        return -1;
    }
    if(turner.run == 1) {
        println("turner busy.");
        return -1;
    }

    turner.run=1;
    // 限位保护
    if (angle < turner.angle_offset.min){
        println("xp_turner_move: dis %.3f bottom_hight %.3f", angle, turner.angle_offset.min);
        angle = turner.angle_offset.min;
    } else if (angle > turner.angle_offset.max){
        angle = turner.angle_offset.max;
    }

    xp_turner_get_location();
    if (angle - turner.angle_offset.current < 0.08 && angle - turner.angle_offset.current > -0.08) {
        turner.run = 0;
        return 0;
    }
    turner.angle_offset.next = angle;
    move_quit=0;
    // 硬限位检查与保护
    if (angle > turner.angle_offset.current && xp_turner_top_limit_touched()) {
        turner.run = 0;
        return -1;
    } else if(angle < turner.angle_offset.current && xp_turner_bottom_limit_touched()) {
        turner.run = 0;
        return -1;
    }
    
    u32 need_time = xp_turner_move_time_estimate(angle);
    // 检查警告
    if(xp_turner_check_warn() != 0 && xp_clear_turner_warn() != 0) {
        println("The turner motor reports a warning and a warning that cannot be cleared.");
        turner.run = 0;
        return -1;
    }

    xp_turner_angle_set(angle);

    aos_msleep(100);
    xp_turner_move_start();

    int start_time = aos_now_ms();
    int abnormal = 0;
    while (1) {
        aos_msleep(500);
        // 限位检查
        if (xp_turner_top_limit_touched() || xp_turner_bottom_limit_touched()) {
            abnormal = -2;
            break;
        }
        // 警告检查
        /*if(xp_turner_check_warn() != 0){
            println("turner stop ,by turner error !");
            abnormal=1;
            break;
        }*/
        if(move_quit == 1) {
            abnormal=1;
            break;
        }
        // 读取编码器信息判断完成
        xp_turner_get_location();
        if ( (turner.motor_angle_offset.current - turner.motor_angle_offset.next) < 20 && (turner.motor_angle_offset.current - turner.motor_angle_offset.next) > -20 ) {
            aos_msleep(500);
            turner.angle_offset.current = angle;
            abnormal = 0;
            break;
        }
        // 超时检查
        if (aos_now_ms() - start_time > need_time) {
            println("turner move timeout.");
            abnormal = -3;
            break;
        }
    }
    xp_turner_move_stop();
    turner.run = 0;
    if(abnormal != 0){
        xp_turner_get_location();
        if(turner.angle_offset.current != angle) {
            println("turner get location (%d) != angle (%d),try again !", turner.angle_offset.current, angle);
            xp_turner_get_location();
        }
    }

    return (abnormal == 0) ? 0 : -1;
}

static int xp_turners_check_zero(void)
{
    // 获取零位传感器或通过电机编码器定位
}

// 检查电机是否报错
static int xp_turner_check_warn(void)
{
    gpio_dev_t gpio;
    u32 value=0;
    gpio.port = 109;
    hal_gpio_input_get(&gpio, &value);
    if(value == 1){
        perr("turner motor have a error !!");
        turner.motor.ops->error_get(&turner.motor, &turner.error);
        return -1;
    }
    return 0;
}

static int xp_turners_set_angle(int angle){ 
	turner.angle_offset.next = angle;
}

// 获取电机编码器的数据
static int xp_turners_get_encode(void)
{
    int num_of_turns = 0, sub_angle = 0;

    // 编码器的数据在驱动层已转换为旋转绝对圈数及不足一圈的旋转角度
    if(turner.motor.ops->encode_get(&turner.motor, &num_of_turns, &sub_angle) != 0) {
        println("error:get turner encode fail~");
        return -1;
    }
    turner.motor_angle_offset.current = num_of_turns * 360 + sub_angle;
    return 0;
}

// 获取举平当前所在的角度
int xp_turner_get_location(void)
{
    int d = 0;

    if(xp_turners_get_encode() !=0 ) return turner.angle_offset.current;
    d = turner.motor_angle_offset.current - turner.motor_angle_offset.origin;
    turner.angle_offset.current = d/TURNERS_ratio;
    println("turners encode %d,origin= %d,location=%d",turner.motor_angle_offset.current, turner.motor_angle_offset.origin,turner.angle_offset.current);
    return turner.angle_offset.current;
}

int xp_turners_get_location_dont_update_value(void){
    int d=0;
    s8 turners_real_time_location = 0;

    if(xp_turners_get_encode() !=0 ) return -10;
    d = turner.motor_angle_offset.current - turner.motor_angle_offset.origin;
    turners_real_time_location = d/TURNERS_ratio;
    println("turners encode %d,origin= %d,real_time_location=%d",turner.motor_angle_offset.current, turner.motor_angle_offset.origin, turners_real_time_location);
    return turners_real_time_location;
}

// 设置举平0点
int xp_turners_as_zero(void){
    char value[20];

    if(xp_turners_get_encode() != 0) return -1;
    turner.motor_angle_offset.origin = turner.motor_angle_offset.current;
    turner.angle_offset.current = 0;
    sprintf(value, "%d", turner.motor_angle_offset.origin);
    if(local_config_kv_set(TURNER_ORIGIN_ANGLE_OFFSET_KEY, value) != xp_false) return -2;
    println("turner.origin set as: %s", value);
    return 0;
}

// 清除举平零点
void xp_turners_zero_clear(void){
    aos_kv_del(TURNER_ORIGIN_ANGLE_OFFSET_KEY);
}

int xp_turner_angle_get(void){
    return turner.angle_offset.current;
}

int xp_turner_move_to_angle(int angle){
    return xp_turners_move(angle,1);
}

void xp_turners_io_event(u32 event){
    static u8 down=1,origin=1,top=1;
    u8 state=0,stop_flag=0;

    state=(event>>TURNERS_io_down)&0x01;
    if(state!=down){
        down=state;
        if(down==TURNERS_io_touch&&turner.angle_offset.current>0&&turner.run==1){
            stop_flag=1;
            println("turners down io touched ! slow stop motor~");
        }
    }
    state=(event>>TURNERS_io_origin)&0x01;
    if(state!=origin){
        origin=state;
        if(origin==TURNERS_io_touch&&turner.check_zero==0&&turner.run==1){
            stop_flag=1;
            println("turners origin io touched ! slow stop motor~");
        }
    }
    state=(event>>TURNERS_io_top)&0x01;
    if(state!=top){
        top=state;
        if(top==TURNERS_io_touch&&turner.angle_offset.current<0&&turner.run==1){
            stop_flag=1;
            println("turners top io touched ! slow stop motor~");
        }
    }

    if(stop_flag==1){
        stop_flag=0;
        move_quit=1;
        turner.motor.ops->rotation_stop(&turner.motor);
        aos_msleep(500);
        turner.motor.ops->rotation_stop(&turner.motor);
    }
}

xp_bool turner_test_enable = xp_false;
void turner_test_function(void *arg)
{
    aos_msleep(5000);
    xp_turners_move(0, 1);
    aos_msleep(1000);
    while(1) {
        if (turner_test_enable) {
            xp_turners_move(90, 1);
            aos_msleep(100);
            xp_turners_move(-10, 1);
            aos_msleep(100);
        } else {
            aos_msleep(500);
        }
    }
}

int xp_turners_debug(char *type,char *fun,char *param){
    s16 angle=0;

    if(strcmp(type,"turners")!=0)return 0;
    if(strcmp(fun,"check_zero")==0){
        if(xp_turners_check_zero()!=0){
            println("turners check zero success!");
        }
        else{
            println("turners check zero fail~");
        }
    }
    else if(strcmp(fun,"support_retry")==0){
        angle=atoi(param);
        if(angle<100&&angle>-100){
            xp_turners_move_support_retry(angle,1,3);
        }
        else{
            println("error:turners move angle over legal area,angle=%d",angle);
        }
    }
    else if(strcmp(fun,"move")==0){
        angle=atoi(param);
        if(angle<100&&angle>-100){
            xp_turners_move(angle,1);
        }
        else{
            println("error:turners move angle over legal area,angle=%d",angle);
        }
    }
    else if(strcmp(fun,"read")==0){
        if(strcmp(param,"location")==0){
            println("turners location=%d",turner.angle_offset.current);
        }
    }
    else if(strcmp(fun,"location")==0){
        xp_turner_get_location();
    }
    else if(strcmp(fun,"set_zero")==0){
        xp_turners_as_zero();
    }
    else if(strcmp(fun,"clear_zero")==0){
        xp_turners_zero_clear();
    }
    else if (strcmp(fun, "turn_forever") == 0) {
        turner_test_enable = atoi(param) == 0 ? xp_false : xp_true;
    }
    else{
        return 0;
    }

    return 1;
}
//end of the file

